#pragma config(Sensor, dgtl1,  RE,             sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  LE,             sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  SO,             sensorSONAR_cm)
#pragma config(Sensor, dgtl7,  RT,             sensorTouch)
#pragma config(Sensor, dgtl8,  LT,             sensorTouch)
#pragma config(Motor,  port2,           RM,            tmotorVex269, openLoop)
#pragma config(Motor,  port3,           LM,            tmotorVex269, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int lspeed=70;
int mspeed=80;

void forward(int revs){
  int turns=revs*360;
    SensorValue[LE]=0;
    SensorValue[RE]=0;
      while(SensorValue[LE]<turns){
    if(SensorValue[RE]>SensorValue[LE]){
      motor[RM]=lspeed;
      motor[LM]=mspeed;
    }
    else if(SensorValue[LE]>SensorValue[RE]){
      motor[RM]=mspeed;
      motor[LM]=lspeed;
    }
    else{
      motor[LM]=mspeed;
      motor[RM]=mspeed;
    }
  }
}

void TR(){
  SensorValue[RE]=0;
  SensorValue[LE]=0;
  while(SensorValue[LE]<170){
    motor[RM]=-mspeed;
    motor[LM]=mspeed;
  }
}

void TL(){
  SensorValue[RE]=0;
  SensorValue[LE]=0;
  while(SensorValue[RE]<170){
    motor[RM]=mspeed;
    motor[LM]=-mspeed;
  }
}

void slightright(){
  SensorValue[RE]=0;
  SensorValue[LE]=0;
  while(SensorValue[RE]>-140){
  motor [RM]=-70;
  motor [LM]=-70;
}
  SensorValue[RE]=0;
  SensorValue[LE]=0;
  while(SensorValue[LE]<150){
  motor [RM]=-70;
  motor [LM]=70;
}
}

void slightleft(){
  SensorValue[RE]=0;
  SensorValue[LE]=0;
  while(SensorValue[LE]>-140){
  motor [RM]=-70;
  motor [LM]=-70;
}
  SensorValue[RE]=0;
  SensorValue[LE]=0;
  while(SensorValue[RE]<150){
  motor [RM]=70;
  motor [LM]=-70;
}
}

void Stop(){
  motor [RM]=0;
  motor [LM]=0;
  wait1Msec(500);
}

void backward(){
  SensorValue[RE]=0;
  SensorValue[LE]=0;
  while(SensorValue[LE]>-25){
    motor[RM]=-70;
    motor[LM]=-70;
  }
}

void checkDirection(){
   Stop();
   TL();
   if(SensorValue[SO]<25){
     TR();
     TR();
   }

}


task main(){
  while(1==1){
    while(SensorValue[SO]>13 || SensorValue[SO]<0){
      if(SensorValue[RT]==0 && SensorValue[LT]==0){
       forward(1);
      }
      else if(SensorValue[RT]==1){
        slightleft();
      }
      else if(SensorValue[LT]==1){
        slightright();
      }
      else if(SensorValue[LT]==1 && SensorValue[RT]==1){
        TR();
      }
    }
    backward();
  checkDirection();
}
}
